#
# Infrared obstacle avoidance sensorr
#
import RPi.GPIO as GPIO
import time
from .motor_driver import *

class Infrared_Avoidance:
    def __init__(self,left_sensor,right_sensor):
        self.Left_Sensor = left_sensor
        self.Right_Sensor =right_sensor
        GPIO.setup(self.Left_Sensor, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(self.Right_Sensor, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        self.Detected_Left_Barrier = False
        self.Detected_Right_Barrier =  False
        self.Detected_Barrier = False

    def detected_barrier(self):
        if (0 == GPIO.input(self.Left_Sensor)):  # 当检测到障碍物时，输出低电平信号
            self.Detected_Left_Barrier = True
            self.Detected_Barrier = True
        else:
            self.Detected_Left_Barrier = False

        if (0 == GPIO.input(self.Right_Sensor)):  # 当检测到障碍物时，输出低电平信号
            self.Detected_Right_Barrier = True
            self.Detected_Barrier = True
        else:
            self.Detected_Right_Barrier = False
        if (self.Detected_Right_Barrier == 0 and self.Detected_Left_Barrier == 0):
            self.Detected_Left_Barrier = False
            self.Detected_Right_Barrier =  False
            self.Detected_Barrier = False

    def destroy(self):
        GPIO.cleanup()                     # Release resource

    def worker(self, motor):
        """thread worker function"""
        while True:
            time.sleep(0.01)
            self.detected_barrier()
            if (motor.MOTOR_STATE == Motor.FORWARD or motor.MOTOR_STATE == Motor.LEFT or motor.MOTOR_STATE == Motor.RIGHT):
                if (self.Detected_Barrier):
                    motor.action(Motor.STOP)
      
if __name__ == '__main__':     # Program start from here
    GPIO.setmode(GPIO.BCM)
    IRroas = Infrared_Avoidance(22,16)
    try:
        while True:
            IRroas.detected_barrier()
            if (IRroas.Detected_Left_Barrier):
                print("Detected Left Barrier!")
            else:
               print("****Left Nothing!******")

            if (IRroas.Detected_Right_Barrier):
                print("Detected Right Barrier!")
            else:
                print("****Right Nothing!******")
    except KeyboardInterrupt:
        IRroas.destroy()
